#include "Model.h"

using namespace std;

Model::Model(PhysicsWorld* pw)
{
	this->pw = pw;
	//dAllocateODEDataForThread(dAllocateMaskAll);
}

void Model::loadModel(QString filename)
{
	QFile modelFile(filename);

	if (!modelFile.open(QIODevice::ReadOnly)) 
		return;

	if (!doc.setContent(&modelFile))
	{ 
		modelFile.close(); 
		return;
	}

	modelFile.close();

	xmlParser();
}

void Model::xmlParser()
{
	QDomElement root = doc.documentElement();

	if(!root.isNull())
		parsingElement(root);
}

void Model::parsingElement(QDomElement de)
{
	de = de.firstChildElement();
	while(!de.isNull())
	{
		QString kinBody = de.attribute("name");
		QDomElement child = de.firstChildElement();
		while (!child.isNull())
		{
			if(!child.tagName().compare("Body", Qt::CaseInsensitive))
			{
				QString bodyName = child.attribute("name");
				QString bodyType = child.attribute("type");

				dBodyID bodyID = dBodyCreate(pw->world);

				dGeomID geomID;

				dVector3 bodyPos = {0.0, 0.0, 0.0};
				dMatrix3 bodyRotMat;
				dRSetIdentity(bodyRotMat);

				dMass m;
				dMassSetZero(&m);
				
				QDomElement bodyAtt = child.firstChildElement();

				while (!bodyAtt.isNull())
				{
					if(!bodyAtt.tagName().compare("Translation", Qt::CaseInsensitive))
					{	
						QStringList transList = bodyAtt.text().split(" ", QString::SkipEmptyParts);
						for(int i = 0; i < 3; i++)
							bodyPos[i] += transList.at(i).toDouble();
					}

					if(!bodyAtt.tagName().compare("Rotation", Qt::CaseInsensitive))
					{	
						QStringList rotList = bodyAtt.text().split(" ", QString::SkipEmptyParts);
						dMatrix3 rot;
						dRFromAxisAndAngle(rot,rotList.at(0).toDouble(), rotList.at(1).toDouble(), rotList.at(2).toDouble(), rotList.at(3).toDouble());
						dMultiply0_333(bodyRotMat,bodyRotMat,rot);
					}

					if(!bodyAtt.tagName().compare("offsetfrom", Qt::CaseInsensitive))
					{
						map<QString, dGeomID>::iterator iter = pw->bodies.find(bodyAtt.text());

						const dReal *offpos = dGeomGetPosition(iter->second);
						const dReal *offrot = dGeomGetRotation(iter->second);

						for(int i = 0; i < 3; i++)
							bodyPos[i] += offpos[i];

						dMultiply0_333(bodyRotMat,bodyRotMat,offrot);
					}

					if(!bodyAtt.tagName().compare("Geom", Qt::CaseInsensitive))
					{
						QString geomType = bodyAtt.attribute("type");

						if(!geomType.compare("cylinder", Qt::CaseInsensitive))
						{
							dReal rads, len;
							QDomElement geomAtt = bodyAtt.firstChildElement();
							while(!geomAtt.isNull())
							{
								if(!geomAtt.tagName().compare("Radius", Qt::CaseInsensitive))
									rads = geomAtt.text().toDouble();
								if(!geomAtt.tagName().compare("Length", Qt::CaseInsensitive))
									len = geomAtt.text().toDouble();

								geomAtt = geomAtt.nextSiblingElement();
							}
							geomID = dCreateCylinder(pw->space,rads, len);
						}

						else if(!geomType.compare("capsule", Qt::CaseInsensitive))
						{
							dReal rads, len;
							QDomElement geomAtt = bodyAtt.firstChildElement();
							while(!geomAtt.isNull())
							{
								if(!geomAtt.tagName().compare("Radius", Qt::CaseInsensitive))
									rads = geomAtt.text().toDouble();
								if(!geomAtt.tagName().compare("Length", Qt::CaseInsensitive))
									len = geomAtt.text().toDouble();

								geomAtt = geomAtt.nextSiblingElement();
							}
							geomID = dCreateCapsule(pw->space,rads, len);
						}
						
						else if(!geomType.compare("sphere", Qt::CaseInsensitive))
						{
							dReal rads;
							QDomElement geomAtt = bodyAtt.firstChildElement();
							while(!geomAtt.isNull())
							{
								if(!geomAtt.tagName().compare("Radius", Qt::CaseInsensitive))
									rads = geomAtt.text().toDouble();

								geomAtt = geomAtt.nextSiblingElement();
							}
							geomID = dCreateSphere(pw->space,rads);
						}

						else if(!geomType.compare("box", Qt::CaseInsensitive))
						{
							QStringList extentList;

							QDomElement geomAtt = bodyAtt.firstChildElement();
							while(!geomAtt.isNull())
							{
								if(!geomAtt.tagName().compare("Extents", Qt::CaseInsensitive))
									extentList = geomAtt.text().split(" ", QString::SkipEmptyParts);

								geomAtt = geomAtt.nextSiblingElement();
							}
							geomID = dCreateBox(pw->space, extentList.at(0).toDouble(), extentList.at(1).toDouble(), extentList.at(2).toDouble());
						}

						if(!kinBody.compare("arm", Qt::CaseInsensitive))
						{
							dGeomSetCategoryBits (geomID, 0x0001); 
							dGeomSetCollideBits (geomID, 0x0001);
							dBodySetGravityMode(bodyID, 0);
							dBodySetFiniteRotationMode (bodyID, 1);
						}
						dGeomSetBody(geomID, bodyID);
					}

					if(!bodyAtt.tagName().compare("Mass", Qt::CaseInsensitive))
					{
						QDomElement massAtt = bodyAtt.firstChildElement();
						QString massType = bodyAtt.attribute("type");

						while(!massAtt.isNull())
						{
							if(!massAtt.tagName().compare("Density", Qt::CaseInsensitive))
							{
								dReal density = massAtt.text().toDouble();

								if(!massType.compare("cylinder", Qt::CaseInsensitive))
								{
									dReal rads,len;
									dGeomCylinderGetParams(geomID, &rads, &len);
									dMassSetCylinder(&m, density, 3, rads, len);
								}
								if(!massType.compare("capsule", Qt::CaseInsensitive))
								{
									dReal rads,len;
									dGeomCapsuleGetParams (geomID,&rads,&len);
									dMassSetCapsule(&m, density, 3, rads, len);
								}
								if(!massType.compare("sphere", Qt::CaseInsensitive))
								{
									dReal rads = dGeomSphereGetRadius(geomID);
									dMassSetSphere(&m, density, rads);
								}
								if(!massType.compare("box", Qt::CaseInsensitive))
								{
									dVector3 sides;
									dGeomBoxGetLengths(geomID, sides);
									dMassSetBox(&m, density, sides[0], sides[1], sides[2]);
								}
							}
							massAtt = massAtt.nextSiblingElement();
						}
						//dMassAdjust(&m, 1.0);
					}
					bodyAtt = bodyAtt.nextSiblingElement();
				}

				dBodySetPosition (bodyID, bodyPos[0], bodyPos[1], bodyPos[2]);
				dBodySetRotation(bodyID, bodyRotMat);
				dBodySetMass (bodyID,&m);

				if(!bodyType.compare("static", Qt::CaseInsensitive))
				{
					const dReal* pos = dBodyGetPosition(bodyID);
					dGeomSetPosition(geomID, pos[0], pos[1], pos[2]);

					const dReal* rot = dBodyGetQuaternion(bodyID);
					dGeomSetQuaternion(geomID, rot);

					dGeomSetBody(geomID, NULL);

					dBodyDestroy(bodyID);
					bodyID = NULL;
				}

				pw->bodies.insert(pair<QString, dGeomID> (bodyName, geomID));
				pw->bodyNum++;
				
				pw->geoms.insert(pair<dGeomID, dBodyID> (geomID, bodyID));
				pw->geomNum++;

				if(!bodyName.compare("magicbox",  Qt::CaseInsensitive))
				{
					pw->magicBoxGeom = geomID;
					pw->magicBoxBody = bodyID;
				}

				if(!bodyName.compare("table",  Qt::CaseInsensitive))
				{
					pw->tableGeom = geomID;
				}

				if(!bodyName.compare("endeffector", Qt::CaseInsensitive))
				{
					pw->endEffector = bodyID;
					pw->fingerBody = bodyID;
					pw->fingerGeom = geomID;
				}
			}

			if(!child.tagName().compare("Joint", Qt::CaseInsensitive))
			{
				QString jointName = child.attribute("name");
				QString jointType = child.attribute("type");

				dJointID jointID;
				
				if(!jointType.compare("hinge", Qt::CaseInsensitive))
					jointID = dJointCreateHinge(pw->world, pw->armgroup);

				QDomElement jointAtt = child.firstChildElement();

				while(!jointAtt.isNull())
				{
					if(!jointAtt.tagName().compare("Body", Qt::CaseInsensitive))
					{	
						QStringList bodyList = jointAtt.text().split(" ", QString::SkipEmptyParts);
						QString body1 = bodyList.at(0);
						QString body2 = bodyList.at(1);

						map<QString, dGeomID>::iterator iterBody = pw->bodies.find(body1);
						map<dGeomID, dBodyID>::iterator iterGeom = pw->geoms.find(iterBody->second);
						dBodyID bodyID1 = iterGeom->second;

						iterBody = pw->bodies.find(body2);
						iterGeom = pw->geoms.find(iterBody->second);
						dBodyID bodyID2 = iterGeom->second;

						dJointAttach (jointID, bodyID1, bodyID2);
					}

					if(!jointAtt.tagName().compare("Offsetfrom", Qt::CaseInsensitive))
					{	
						QString bodyName = jointAtt.text();
						map<QString, dGeomID>::iterator iterBody = pw->bodies.find(bodyName);
						map<dGeomID, dBodyID>::iterator iterGeom = pw->geoms.find(iterBody->second);
						dBodyID bodyID = iterGeom->second;
						dGeomID geomID = iterBody->second;
						const dReal* pos = dBodyGetPosition(bodyID);
						int geomType = dGeomGetClass(geomID);
						if(geomType == dSphereClass)
						{
							dReal radius = dGeomSphereGetRadius(geomID);
							dJointSetHingeAnchor(jointID, pos[0], pos[1], pos[2]-radius);
						}

						else if(geomType == dCapsuleClass)
						{
							dReal radius, length;
							dGeomCapsuleGetParams(geomID, &radius, &length);
							dJointSetHingeAnchor(jointID, pos[0], pos[1], pos[2] - radius - length / 2);
						}
					}

					if(!jointAtt.tagName().compare("axis", Qt::CaseInsensitive))
					{	
						QStringList axisList = jointAtt.text().split(" ", QString::SkipEmptyParts);
						dJointSetHingeAxis(jointID, axisList.at(0).toDouble(), axisList.at(1).toDouble(), axisList.at(2).toDouble());
					}

					if(!jointAtt.tagName().compare("limitsdeg", Qt::CaseInsensitive))
					{	
						QStringList degList = jointAtt.text().split(" ", QString::SkipEmptyParts);
						dJointSetHingeParam(jointID, dParamLoStop, DEGTORAD(degList.at(0).toDouble()));
						dJointSetHingeParam(jointID, dParamHiStop,  DEGTORAD(degList.at(1).toDouble()));
					}

					jointAtt = jointAtt.nextSiblingElement();
				}

				//dJointSetHingeParam(jointID, dParamCFM, 1e-6);
				//dJointSetHingeParam(jointID, dParamERP, 0.99);
				//dJointSetHingeParam(jointID, dParamBounce, 1e-4);

				pw->armJoints.insert(pair<QString, dJointID> (jointName, jointID));
				pw->armJointNum++;

				if(!jointName.compare("joint2", Qt::CaseInsensitive))
				{
					pw->wristJoint = jointID;
				}
			}
			child = child.nextSiblingElement();
		}
		de = de.nextSiblingElement();
	}
}